#include "PLT_HEADS.h"
#include "TK_PID.h"
/*---------------------------------------------------------------------------------------------------------*/
PID_HandleTypeDef TargetPID;
#define SCALE_A 1000
#define SCALE_B 10
#define Pumparrylength                  ARRAY_SIZE(Pump_Coefficient)
code const PumpCoeff Pump_Coefficient[] = // a*x^2 + b*x + c   pumpduty 温差
{
    //a      b    c   PumpDuty 温差
    { 156,  368,  126 ,  80, 54,},       //    F(x) = +0.15621633268692037*x*x+36.8401206636501*x+125.85649644473155
    { 18 ,  385,  46  ,  75, 58,},       //    F(x) = +0.01831501831501854*x*x+38.55311355311354*x+45.71428571428585
    { 438,  348,  54  ,  70, 62,},       //    F(x) = +0.043777360850531535*x*x+34.784240150093815*x+54.440275171982364
    { 2  ,  343,  35  ,  65, 67,},       //    F(x) = +0.002394636015325774*x*x+34.26484674329501*x+35.488505747126545
    { -2 ,  324,  -1  ,  60, 76,},       //    F(x) = -0.0019765970904492202*x*x+32.4537476280835*x-1.1424731182796557
    { -20,  317,  -45 ,  55, 80,},       //    F(x) = -0.019841269841269077*x*x+31.666666666666618*x-44.503968253967564
    { -36,  285,  -32 ,  50, 90,},       //    F(x) = -0.036075036075035524*x*x+28.463203463203428*x-31.818181818181277
};
/*===========================================================================================================
Function    : findCoefficientIndex
Input       : 
Output      : 
Description : 
===========================================================================================================*/
unsigned char findCoefficientIndex(unsigned int errorValue,PumpCoeff *hpumpcoeff) 
{
    unsigned char i,j,k;
    k = (unsigned char)Pumparrylength;
    // 检查表格有效性
    if (k <= 0)
    {
        return 0;
    }
    if(errorValue >= hpumpcoeff[(k - 1)].tempDiff)
    {
        j = (k - 1);
    }
    else
    {
        for(i = 0;i < k;i++)
        {
            if(errorValue < hpumpcoeff[i].tempDiff)
            {
                j = i;
                break;
            }
        }
    }
    if(j >= Pumparrylength)
    {
        j = Pumparrylength - 1;
    }
    return j;
}
/*===========================================================================================================
Function    : Power_INIT
Input       : 当前值，目标值
Output      : 功率输出
Description : 首次输出功率初始化 使用近似曲线计算输出功率初始值（已废弃）
===========================================================================================================*/
unsigned int Power_Init(PumpCoeff *hpumpcoeff,unsigned int u16_PresentValue,unsigned int u16_TargetValue)
{
    unsigned char j;
    unsigned int u16_ErrorValue,u16_powerout;
    signed long s32_data[3];
    if (hpumpcoeff == ((void*)0))
    {
        return 0; // 或者触发错误处理
    }
    if(u16_PresentValue >= u16_TargetValue)//当前值大于等于目标值 输出功率0
    {
        u16_powerout = 0;
    }
    else
    {
        u16_ErrorValue = u16_TargetValue - u16_PresentValue;
        j = findCoefficientIndex(u16_ErrorValue,hpumpcoeff);
        // a*x^2 + b*x + c 
        s32_data[0] = (signed long)(u16_ErrorValue*u16_ErrorValue);
        s32_data[0] = (signed long)(s32_data[0]*hpumpcoeff[j].a);
        s32_data[0] = s32_data[0]/SCALE_A;
        s32_data[1] = (signed long)(u16_ErrorValue*hpumpcoeff[j].b);
        s32_data[1] = s32_data[1]/SCALE_B;
        s32_data[2] = (signed long)hpumpcoeff[j].c + s32_data[0] + s32_data[1];
        u16_powerout = (unsigned int)s32_data[2];
    }
    return u16_powerout;
}
/*===========================================================================================================
Function    : Pump_INIT
Input       : 当前温度值，目标温度值
Output      : 水泵占空比 (0-100)
Description : 根据温差曲线计算水泵初始占空比
===========================================================================================================*/
unsigned char Pump_Init(PumpCoeff *hpumpcoeff,unsigned int u16_PresentValue,unsigned int u16_TargetValue)
{
    unsigned char i,u8_pumpout;
    unsigned int u16_ErrorValue;
    if (hpumpcoeff == ((void*)0))
    {
        return 0; // 或者触发错误处理
    }
    if(u16_PresentValue >= u16_TargetValue)//当前值大于等于目标值 输出功率0
    {
        u8_pumpout = hpumpcoeff[0].pumpDuty;
    }
    else
    {
        u16_ErrorValue = u16_TargetValue - u16_PresentValue;
        i = findCoefficientIndex(u16_ErrorValue,hpumpcoeff);
        u8_pumpout = hpumpcoeff[i].pumpDuty;
    }
    return u8_pumpout;
}
/*===========================================================================================================
Function    : PID_Calculate
Input       : PID句柄，当前温度值
Output      : 加热功率输出占空比 (0-2000)
Description : 增量式PID计算，根据当前温度与目标温度的偏差调整水泵占空比
===========================================================================================================*/
unsigned int PID_Calculate(PID_HandleTypeDef *hpid, unsigned int u16_PresentValue)
{
    signed int s16_out;
    hpid->actual_value = u16_PresentValue;
    //计算当前误差
    hpid->err = hpid->set_value - hpid->actual_value;
    //计算当前增量输出
    s16_out = hpid->kp*(hpid->err - hpid->err_next) + hpid->ki*hpid->err + hpid->kd*(hpid->err - (2*hpid->err_next) + hpid->err_last);
    //更新当前输出
    hpid->out += s16_out;
    // 输出限幅
    if (hpid->out > MAXOUTVALUE) hpid->out = MAXOUTVALUE;
    else if (hpid->out < MINOUTVALUE) hpid->out = MINOUTVALUE;
    // 更新历史误差    
    hpid->err_last    = hpid->err_next;
    hpid->err_next    = hpid->err;
    return hpid->out;
}
/*===========================================================================================================
Function    :
Input       :
Output      :
Description :增量式PID算法初始化
===========================================================================================================*/
void PID_Init(PID_HandleTypeDef *hpid, unsigned int u16_PresentValue, unsigned int u16_TargetValue)
{
    hpid->set_value    = u16_TargetValue;
    hpid->actual_value = u16_PresentValue;
    hpid->err         = 0.0;
    hpid->err_next    = 0.0;
    hpid->err_last    = 0.0;
    hpid->kp          = PID_KP;
    hpid->ki          = PID_KI;
    hpid->kd          = PID_KD;
    hpid->out         = Power_Init(Pump_Coefficient,hpid->actual_value, hpid->set_value);
    hpid->pumpDuty    = Pump_Init(Pump_Coefficient,hpid->actual_value, hpid->set_value);
}